function myRectRGBD(rgb_img_file, depth_img_file)
%function [rgb_rect, depth_rect] = rect_RGBD(rgb_img_file, depth_img_file)

%INPUT: fc: Camera focal length
%        cc: Principal point coordinates
%        alpha_c: Skew coefficient
%        kc: Distortion coefficients
%        KK: The camera matrix (containing fc and cc)

nx = 640;
ny = 480;

% RGB cameara paramters
fc_rgb = [ 529.215080982932 ; 525.563936300574 ] % RGB Focal Length 
cc_rgb = [ 328.942720287592 ; 267.480681718715 ] % RGB Principal point
alpha_c_rgb = [ 0.00000 ] % RGB Skew
kc_rgb = [ 0.264516223330095 ; -0.839907494246208 ; -0.001992230217369 ; 0.001437199593290 ; 0.911924650787138 ] % RGB Distortion
KK_rgb = [fc_rgb(1) alpha_c_rgb*fc_rgb(1) cc_rgb(1);0 fc_rgb(2) cc_rgb(2) ; 0 0 1];

%fc_rgb = [ 534.0716 ; 532.5928 ] % RGB Focal Length 
%cc_rgb = [ 317.8926 ; 251.1865 ] % RGB Principal point
%alpha_c_rgb = [ 0.00000 ] % RGB Skew
%kc_rgb = [ 0.2064374000 ; -0.3961256 ; -0.001762185 ; 0.002132843 ; 0.000000001 ] % RGB Distortion
%KK_rgb = [fc_rgb(1) alpha_c_rgb*fc_rgb(1) cc_rgb(1);0 fc_rgb(2) cc_rgb(2) ; 0 0 1];



% Depth cameara paramters
fc_depth = [ 594.214342119232 ; 591.040536968707 ] % Depth Focal Length 
cc_depth = [ 339.307809753003 ; 242.739137617516 ] % Depth Principal point
alpha_c_depth = [ 0.00000 ] % Depth Skew
kc_depth = [ -0.263864897531288 ; 0.999668321637297 ; -0.000762758621436 ; 0.005035094009081 ; -1.305362808997630 ] % Depth Distortion
KK_depth = [fc_depth(1) alpha_c_depth*fc_depth(1) cc_depth(1);0 fc_depth(2) cc_depth(2) ; 0 0 1];

rgb = double(imread(rgb_img_file));
depth = double(imread(depth_img_file));

[rgb_part1] = rect(rgb(:,:,1),eye(3),fc_rgb,cc_rgb,kc_rgb,alpha_c_rgb,KK_rgb);
[rgb_part2] = rect(rgb(:,:,2),eye(3),fc_rgb,cc_rgb,kc_rgb,alpha_c_rgb,KK_rgb);
[rgb_part3] = rect(rgb(:,:,3),eye(3),fc_rgb,cc_rgb,kc_rgb,alpha_c_rgb,KK_rgb);

rgb2 = ones(ny, nx,3);
rgb2(:,:,1) = rgb_part1;
rgb2(:,:,2) = rgb_part2;
rgb2(:,:,3) = rgb_part3;

image(uint8(rgb2));

[depth2] = rect(depth,eye(3),fc_depth,cc_depth,kc_depth,alpha_c_depth,KK_depth);


imwrite(uint8(round(rgb2)),'rgb_rect.jpg', 'jpg');
imwrite(uint8(round(depth2)),gray(256),'depth_rect.jpg', 'jpg');



